#!/usr/bin/env python3
# coding:utf-8
# author: Keben Hong
# first created on 2024.3.13
# 简介: 将xgc-rviz的单点话题以PoseArray形式储存，并固定频率进行转发

import os
import yaml
import numpy as np

import rospy
from geometry_msgs.msg import PoseStamped, Pose, TransformStamped, PoseArray
import tf2_ros
from visualization_msgs.msg import Marker
from tf.transformations import quaternion_from_euler

class TargetObject:
    def __init__(self):
        self.pose_list = PoseArray()
        self.pose = PoseStamped()
        self.pub = rospy.Publisher("/target",PoseArray,queue_size=10)
        self.sub  = rospy.Subscriber("/move_base_simple/goal",PoseStamped,self.pose_cb,queue_size=10)
        self.marker_pub = rospy.Publisher("/marker",Marker,queue_size=10)

    def pose_cb(self, data):
        self.pose.pose = data.pose
        print(data.pose)
        self.pose_list.poses.append(self.pose.pose)
        marker = Marker()
        marker.header.frame_id = data.header.frame_id
        marker.type = Marker.MESH_RESOURCE
        marker.mesh_resource = "/home/hkb/myros/target_ws/src/target_visualize/model/meshes/tree.dae"  # 更新为实际的模型路径
        marker.pose.position.x, marker.pose.position.y, marker.pose.position.z = data.pose.position.x, data.pose.position.y, data.pose.position.z

        marker.scale.x, marker.scale.y, marker.scale.z = 1.0, 1.0, 1.0
        marker.color.r, marker.color.g, marker.color.b, marker.color.a = 1.0, 1.0, 1.0, 1.0
        self.marker_pub.publish(marker)

    def publish_pose(self):
        self.pub.publish(self.pose_list)

if __name__ == "__main__":
    targets = TargetObject()
    rospy.init_node("target_transfer")
    rate = rospy.Rate(10)
    # targets.pose_list.poses = []
    while not rospy.is_shutdown():
        targets.publish_pose()
        rate.sleep()
